#include "misc.h"

void transformPointsForward(vector<Point2f>&origin, const Matx33d &H,bool isBox,const Rect &box)
{
	vector<Point2f> dst;
    const double *aa = H.val;
	dst = origin;
	for (int i = 0; i<dst.size(); i++) {
		auto &a = dst[i];
		auto &b = origin[i];
		double s = aa[6] * a.x + aa[7] * a.y + aa[8];
		b.x = (aa[0] * a.x + aa[1] * a.y + aa[2]) / s;
		b.y = (aa[3] * a.x + aa[4] * a.y + aa[5]) / s;
	}
}


void transformPointsInverse(vector<Point2f>&origin, const Matx33d &H,bool isBox,const Rect &box)
{
	Matx33d invH;
	invert(H, invH);
	vector<Point2f> dst;
	dst = origin;
    const double *aa = invH.val;
	for (int i = 0; i<dst.size(); i++) {
		auto &a = dst[i];
		auto &b = origin[i];
		double s = aa[6] * a.x + aa[7] * a.y + aa[8];
		b.x = (aa[0] * a.x + aa[1] * a.y + aa[2]) / s;
		b.y = (aa[3] * a.x + aa[4] * a.y + aa[5]) / s;
	}
}




void transformPointsForward(const vector<KeyPoint>&origin, vector<KeyPoint> &dst, const Matx33d&H)
{
	const double *aa = H.val;
	dst = origin;
	for (int i = 0; i<dst.size(); i++) {
		auto &a = dst[i].pt;
		double s = aa[6] * a.x + aa[7] * a.y + aa[8];
		a.x = (aa[0] * a.x + aa[1] * a.y + aa[2]) / s;
		a.y = (aa[3] * a.x + aa[4] * a.y + aa[5]) / s;
	}
}
void transformPointsInverse(const vector<KeyPoint> &origin, vector<KeyPoint>&dst, const Matx33d&H)
{
	Matx33d invH;
	invert(H, invH);
	dst = origin;
	const double *aa = invH.val;
	for (int i = 0; i<dst.size(); i++) {
		auto &a = dst[i].pt;
		double s = aa[6] * a.x + aa[7] * a.y + aa[8];
		a.x = (aa[0] * a.x + aa[1] * a.y + aa[2]) / s;
		a.y = (aa[3] * a.x + aa[4] * a.y + aa[5]) / s;
	}
}



void warpScale(const Mat&src, Mat&dst, const Matx33d H, float _scale, int level)
{
	double scale = 1 / pow(_scale, level - 1);
	Matx33d s =Matx33d::eye();
	s(0, 0) = scale;
	s(1, 1) = scale;
	s = H*s;
	warpPerspective(src, dst, s, cv::Size(src.cols*scale,src.rows*scale));
}



void setPyramid(const Mat &img, const int levelsNum, const float scaleFactor, vector<Mat> &imagePyramid)
{
	imagePyramid.resize(levelsNum);

	const int border = 5;
	const int firstLevel = 0;
	for (int level = 0; level < levelsNum; ++level)
	{
		float scale = 1 / pow(scaleFactor,level);
		cv::Size sz(cvRound(img.cols*scale), cvRound(img.rows*scale));
		cv::Size wholeSize(sz.width + border * 2, sz.height + border * 2);
		Mat temp(wholeSize, img.type()), masktemp;
		imagePyramid[level] = temp(Rect(border, border, sz.width, sz.height));

		// Compute the resized img
		if (level != firstLevel)
		{
			if (level < firstLevel)
			{
				resize(img, imagePyramid[level], sz, 0, 0, INTER_LINEAR);
			}
			else
			{
				resize(imagePyramid[level - 1], imagePyramid[level], sz, 0, 0, INTER_LINEAR);
			}

			copyMakeBorder(imagePyramid[level], temp, border, border, border, border,
				BORDER_REFLECT_101 + BORDER_ISOLATED);
		}
		else
		{
			copyMakeBorder(img, temp, border, border, border, border,
				BORDER_REFLECT_101);
		}
	}
}
